#include <uorb_data.hpp>
#include <uorb_utils.hpp>

static UorbUtils* s_UorbUtils = nullptr;

UorbUtils* UorbUtils::Instance()
{
	if (s_UorbUtils == nullptr)
	{
		s_UorbUtils = new UorbUtils();
	}
	return s_UorbUtils;
}

UorbUtils::UorbUtils()
	: m_SemLock(1)
{
}

UorbUtils::~UorbUtils()
{
}

bool UorbUtils::Create(const char* uorbName, int dataSize)
{
	if (dataSize <= 0)
	{
		return false;
	}

	m_SemLock.Wait();
	UorbData* value = m_UorbMap[uorbName];
	m_SemLock.Post();
	if (value != nullptr)
	{
		return false;
	}

	// 创建uorb数据
	UorbData* uorb	  = new UorbData();
	uorb->m_ReaderCnt = 0;
	uorb->m_SubCnt	  = 0;
	uorb->m_DataSize  = dataSize;
	uorb->m_Data	  = malloc(dataSize);
	// 更新所有订阅者都变成是已读状态
	memset(uorb->m_SubList, 0xFF, sizeof(uint32_t) * UORB_SUB_SIZE);
	// 将新创建的uorb插入到map中
	m_UorbMap[uorbName] = uorb;

	return true;
}

bool UorbUtils::Publish(const char* uorbName, char* data, int size)
{
	m_SemLock.Wait();
	UorbData* uorb = m_UorbMap[uorbName];
	m_SemLock.Post();
	if (uorb == nullptr)
	{
		LOG("cannot publish uorb\n");
		return false;
	}

	if (size != uorb->m_DataSize)
	{
		LOG("publish uorb size error.\n");
		return false;
	}

	uorb->m_SemW.Wait();
	// 复制数据
	memcpy(uorb->m_Data, data, size);
	// 更新所有订阅者都变成是未读状态
	memset(uorb->m_SubList, 0, sizeof(uint32_t) * UORB_SUB_SIZE);
	uorb->m_SemW.Post();

	return true;
}

int UorbUtils::Subscribe(const char* uorbName)
{
	m_SemLock.Wait();
	UorbData* uorb = m_UorbMap[uorbName];
	m_SemLock.Post();
	if (uorb == nullptr)
	{
		return -1;
	}

	int subId = -1;
	uorb->m_SemW.Wait();
	if (uorb->m_SubCnt < (int)(UORB_SUB_SIZE * 8 * sizeof(uint32_t)))
	{
		subId = uorb->m_SubCnt++;
	}
	uorb->m_SemW.Post();

	return subId;
}

// 订阅uORB，如果订阅失败则创建uORB
int UorbUtils::SubscribeAuto(const char* uorbName, int size)
{
	// 订阅uorb
	int subId = Subscribe(uorbName);
	// 如果没有订阅成功
	if (subId < 0)
	{
		// 创建uorb
		if (!Create(uorbName, size))
		{
			return -1;
		}

		// 再次订阅
		subId = Subscribe(uorbName);
		if (subId < 0)
		{
			return -1;
		}
	}

	return subId;
}

bool UorbUtils::Copy(const char* uorbName, int subId, char* data, int size)
{
	m_SemLock.Wait();
	UorbData* uorb = m_UorbMap[uorbName];
	m_SemLock.Post();
	if (uorb == nullptr)
	{
		LOG("cannot copy uorb %s\n", uorbName);
		return false;
	}

	if (size != uorb->m_DataSize)
	{
		LOG("copy uorb size error");
		return false;
	}

	if (subId < uorb->m_SubCnt && subId < (int)(UORB_SUB_SIZE * 8 * sizeof(uint32_t)))
	{
		int u32_index = subId / (sizeof(uint32_t) * 8);
		int bit_index = subId % (sizeof(uint32_t) * 8);
		// 如果是已读状态则返回
		if ((uorb->m_SubList[u32_index] >> bit_index) & 0x1)
		{
			return false;
		}
	}

	uorb->m_SemR.Wait();
	if (uorb->m_ReaderCnt == 0)
	{
		// 只有最先读取的线程需要等待写入信号量
		uorb->m_SemW.Wait();
	}
	// 读者增加
	uorb->m_ReaderCnt++;
	uorb->m_SemR.Post();

	// 复制数据
	memcpy(data, uorb->m_Data, size);
	// 更新已读状态
	if (subId < uorb->m_SubCnt && subId < (int)(UORB_SUB_SIZE * 8 * sizeof(uint32_t)))
	{
		int u32_index = subId / (sizeof(uint32_t) * 8);
		int bit_index = subId % (sizeof(uint32_t) * 8);
		// 更新已读状态
		uorb->m_SubList[u32_index] |= (0x1 << bit_index);
	}

	uorb->m_SemR.Wait();
	// 读者减少
	uorb->m_ReaderCnt--;
	if (uorb->m_ReaderCnt == 0)
	{
		// 只有最后读取的线程需要释放写入信号量
		uorb->m_SemW.Post();
	}
	uorb->m_SemR.Post();

	return true;
}